function ddq = rocketDynamics(~,q,dq,u,P) 
%ddq = ROCKETDYNAMICS(t,q,dq,u,P)
% 
%FUNCTION:  This function computes the dynamics of a double
%    pendulum, and is designed to be called from ode45. The
%    model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
% 
%INPUTS: 
%    t = time. 
%    q = [3xn] matrix of states.
%    dq = [3xn] matrix of state rates.
%    u = [2xn] matrix of inputs.
%    P = struct of parameters
%OUTPUTS: 
%    dq = [3xn] matrix of state accelerations
% 
%NOTES:
%    This file was automatically generated by writeDynamics

m = P.m; %rocket mass
G  = P.G ; %gravity constant
mPlanet = P.mPlanet; %mass of the planet
l = P.l; %rocket length
d = P.d; %rocket engine eccentricity

r = q(1,:); %distance between planet CoM and rocket CoM
th1 = q(2,:); %angle of vector between planet and rocket
th2 = q(3,:); %absolute orientation of the rocket

dr = dq(1,:); %distance between planet CoM and rocket CoM - rate
dth1 = dq(2,:); %angle of vector between planet and rocket -rate
dth2 = dq(3,:); %absolute orientation of the rocket - rate

u1 = u(1,:); %Rocket engine one force
u2 = u(2,:); %Rocket engine two force

ddr =  (r.^2.*u1.*cos(th1).*cos(th2) + r.^2.*u2.*cos(th1).*cos(th2) + r.^2.*u1.*sin(th1).*sin(th2) + r.^2.*u2.*sin(th1).*sin(th2) + dth1.^2.*m.*r.^3.*cos(th1).^2 + dth1.^2.*m.*r.^3.*sin(th1).^2 - G.*m.*mPlanet.*cos(th1).^2 - G.*m.*mPlanet.*sin(th1).^2)./(m.*r.^2.*(cos(th1).^2 + sin(th1).^2));
ddth1 =  -(u1.*cos(th2).*sin(th1) - u1.*cos(th1).*sin(th2) - u2.*cos(th1).*sin(th2) + u2.*cos(th2).*sin(th1) + 2.*dr.*dth1.*m.*cos(th1).^2 + 2.*dr.*dth1.*m.*sin(th1).^2)./(m.*r.*(cos(th1).^2 + sin(th1).^2));
ddth2 =  (12.*d.*(u1 - u2))./(l.^2.*m);

ddq = [ddr;ddth1;ddth2];
end 
